
/*
   DRV8833 for ToyCar 

   简易电机阿克曼转向(ackermann  steering)
   * Motor 1 : Forward/Backward
   * Motor 2 : Turn left/right/center
*/
#ifndef DRV8833_TOYCAR_H
#define DRV8833_TOYCAR_H

#define FORWARD_PIN 0   // AIN1 黑色电机线
#define BACKWARD_PIN 1  // AIN2 绿色电机线

#define TURN_LEFT_PIN 7   // BIN2 绿色电机线
#define TURN_RIGHT_PIN 6  //  BIN1黑色电机线

// Setting PWM properties
const int freq = 30000;
const int resolution = 8;

enum ROBOT_ACTION_CODE {
  ROBOT_FORWARD = 1,
  ROBOT_BACKWARD,
  ROBOT_STOP,
  // turn
  ROBOT_TURN_LEFT,
  ROBOT_TURN_RIGHT,
  ROBOT_CENTER,
  // speed
  ROBOT_SPEED_UP,
  ROBOT_SPEED_DOWN,
};

// robot running state
enum ROBOT_ACTION_CODE robot_state = ROBOT_STOP;
enum ROBOT_ACTION_CODE cur_robot_cmd = ROBOT_STOP;

// PWM - speed
int robot_speed = 220;
int turn_speed = 255;
const int robot_speed_step = 10;

void robot_forward() {
  ledcWrite(FORWARD_PIN, robot_speed);
  ledcWrite(BACKWARD_PIN, 0);
}

void robot_backward() {
  ledcWrite(FORWARD_PIN, 0);
  ledcWrite(BACKWARD_PIN, robot_speed);
}

void robot_stop() {
  ledcWrite(FORWARD_PIN, 0);
  ledcWrite(BACKWARD_PIN, 0);
}

void robot_turn_left() {
  ledcWrite(TURN_LEFT_PIN, turn_speed);
  ledcWrite(TURN_RIGHT_PIN, 0);
}

void robot_turn_right() {
  ledcWrite(TURN_LEFT_PIN, 0);
  ledcWrite(TURN_RIGHT_PIN, turn_speed);
}

void robot_center() {
 //  简易阿克曼转向(ackermann  steering)。转向电机停止，转向的机械结构自动居中
  ledcWrite(TURN_LEFT_PIN, 0);
  ledcWrite(TURN_RIGHT_PIN, 0);
}

void running_with_new_speed() {
  switch (robot_state) {
    case ROBOT_FORWARD:
      robot_forward();
      break;
    case ROBOT_BACKWARD:
      robot_backward();
      break;
    default:
      break;
  }
}

void robot_speed_up() {
  robot_speed += robot_speed_step;
  if (robot_speed > 255) {
    robot_speed = 255;
  }
  running_with_new_speed();
}

void robot_speed_down() {
  robot_speed -= robot_speed_step;
  if (robot_speed < 0) {
    robot_speed = 0;
  }
  running_with_new_speed();
}

void robot_action(ROBOT_ACTION_CODE robot_cmd) {
  switch (robot_cmd) {
    case ROBOT_FORWARD:
      robot_forward();
      robot_state = ROBOT_FORWARD;
      break;
    case ROBOT_BACKWARD:
      robot_backward();
      robot_state = ROBOT_BACKWARD;
      break;
    case ROBOT_STOP:
      robot_stop();
      robot_state = ROBOT_STOP;
      break;
    case ROBOT_TURN_LEFT:
      robot_turn_left();
      break;
    case ROBOT_TURN_RIGHT:
      robot_turn_right();
      break;
    case ROBOT_CENTER:
      robot_center();
      break;
    case ROBOT_SPEED_UP:
      robot_speed_up();
      break;
    case ROBOT_SPEED_DOWN:
      robot_speed_down();
      break;
    default:
      break;
  }  // switch
};

void setup_drv8833() {
  pinMode(FORWARD_PIN, OUTPUT);
  pinMode(BACKWARD_PIN, OUTPUT);
  pinMode(TURN_RIGHT_PIN, OUTPUT);
  pinMode(TURN_LEFT_PIN, OUTPUT);

  // configure LED PWM functionalitites
  ledcAttach(FORWARD_PIN, freq, resolution);
  ledcAttach(BACKWARD_PIN, freq, resolution);

  ledcAttach(TURN_RIGHT_PIN, freq, resolution);
  ledcAttach(TURN_LEFT_PIN, freq, resolution); 
  robot_stop();
  robot_center();

}

void loop_drv8833() {
}

#endif /* DRV8833_H */